//
// Created by ze on 2023/11/9.
//

#include "UPRE_PATH.h"
#include "UPRE_PID.h"
#include "UPRE_MOVEBASE.h"
#include "UPRE_LOCATION.h"
#include "UPRE_ROBOT_TASK.h"
#include "Kalmanfilter.h"
/********************结构***********************
* 1. 相应路径先初始化
* 2. 走路径即可
***********************************************/

// 默认路径配置
// 默认起始速度
static const int DEFAULT_START_SPEED = 200;
// 默认最大车速
static const int DEFAULT_MAX_SPEED = 500;
// 默认最终车速
static const int DEFAULT_END_SPEED = 0;
// 默认加速百分比
static const uint8_t DEFAULT_ACCELERATE_PER = 40;
// 默认减速百分比
static const uint8_t DEFAULT_DESCEND_PER = 40;
// 达到目标Yaw值占路径百分比
static const uint8_t DEFAULT_YAW_ADJUST_PER = 20;
//默认减速距离
static const uint32_t DEFAULT_DECELERATION_DISTANCE = 800;

// variable
//设置的路径 ID
static int pathID = 0;
//正在走的路径 ID
static int goPathID = 0;
//路径状态
static PATH_STATE pathState = PATH_ERROR;
//路径的平移速度
static vector pathVel = { 0.f };
// 路径配置参数
static PathParam PathCfg = { 0 };
//直线路径结束坐标
Pose Print_End_Pose = {0};

//路径点
point Path_PathPoint[UPRE_PATH_PATHNUM];

float path_percentNow = 0;
uint8_t path_PathState = 0;
uint8_t path_type = 0;
/*

*/
/*********************************************************
 * 函数名称：GoStraightLine
 * 输入：起始位姿和结束位姿
 * 功能：静态跟踪直线路径
 * 参数：无
 * 输出：_Vector 运动的总速度角度
 * 返回值：路径运动状态
 * *******************************************************/


static PATH_STATE GoStraightLine()
{
    // 直线
    static float b1 = 0.f;
    // 垂线
    static float b2 = 0.f;
    //垂线角度
    static float vertical_dir = 0.f;
    //垂线斜率
    static float verticle_k = 0.f;
    // 直线角度
    static float line_dir = 0.f;
    // 直线斜率
    static float line_k = 0.f;
    // 直线与其斜线的差值
    static float line_ver_k = 0.f;


    //直线长度
    static float Distance_Line;

    // 路径分配的速度
    static vector pathAssignmentVel = { 0.f };

    //目标直线向量
    static point Target_Line;
    //当前坐标到直线起始点坐标
    static point NowToStartvector;
    //当前点到起始点与目标直线的投影
    static float ProjectionOnTargetLinevector = 0.0;
    //调节点
    static Pose Pose_Adjust;
    //当前坐标与调节点
    static point NowToAdjustPosevector;
    //路径百分比
    static float PercentNow = 0.0;
    //路径是否到达100%
    static bool pathFinishFlag = false;
//    static bool Once = true;

/********************************************************************************************************/

    path_percentNow = PercentNow;
    //判断是否更换过路径
    if (goPathID == pathID)
    {
#if Printf_Percent == 1
        printf("Percent:%f\n", PercentNow);
#endif

        if (!pathFinishFlag)
        {
            NowToStartvector.x = Robot_Pose.wx - PathCfg.startPose.wx;
            NowToStartvector.y = Robot_Pose.wy - PathCfg.startPose.wy;

            ProjectionOnTargetLinevector = (NowToStartvector.x * Target_Line.x + NowToStartvector.y * Target_Line.y) / Distance_Line;

            PercentNow = To2DecimalPlaces(ProjectionOnTargetLinevector * 100 / Distance_Line);

            Print_End_Pose = PathCfg.endPose;
            if (PercentNow < 0)
            {
//                printf("EEEEEEEEEEEEEEEEEEERRRRRRRRRRRRRRRRRRRRRRRRRRR!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\r\n");

                PercentNow = 0.f;
//                if(Once)
//                {
//                    Once = false;
                Pose_Adjust = PathCfg.startPose;
//                }
            }
            if (PercentNow >= 98.f)
            {
                PercentNow = 100.f;
                Pose_Adjust = PathCfg.endPose;
                pathFinishFlag = true;
            }
            else if(Chassis_Sense_Bool.Forced_Close_Path)
            {
//                printf("Start_Forced_Close_Path%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r\n");
                PercentNow = 100.f;
                pathFinishFlag = true;
                Chassis_Sense_Bool.Forced_Close_Path = false;
                Chassis_Sense_Bool.Forced_Close_Path_OK = false;
            }
            else
            {
                b2 = Robot_Pose.wy - verticle_k * Robot_Pose.wx;

                //找到调节点
                Pose_Adjust.wx = (b2 - b1) / line_ver_k;
                Pose_Adjust.wy = (b2 * line_k - b1 * verticle_k) / line_ver_k;
                if (PercentNow <= (float)PathCfg.yawAdjustPercent)
                {
                    Pose_Adjust.yaw = (PathCfg.endPose.yaw - PathCfg.startPose.yaw) * PercentNow / (float)(PathCfg.yawAdjustPercent) + PathCfg.startPose.yaw;
                }
                else
                {
                    Pose_Adjust.yaw = PathCfg.endPose.yaw;
                }

            }
//分配路径速度
            if(PercentNow >= 100.f)
            {
                pathAssignmentVel.size = 0;
            }
            else if (PercentNow <= PathCfg.acceleratePercent && PathCfg.acceleratePercent != 0)
            {
                pathAssignmentVel.size = sqrtf( (PercentNow / PathCfg.acceleratePercent) * (square(PathCfg.maxSpeed) - square(PathCfg.startSpeed))
                                                + square(PathCfg.startSpeed));
            }
            else if ((100 - PercentNow) <= PathCfg.descentPercent && PathCfg.descentPercent != 0)
            {
                pathAssignmentVel.size = sqrtf(((100.f - PercentNow) / PathCfg.descentPercent) * (square(PathCfg.maxSpeed) - square(PathCfg.endSpeed))
                                               + square(PathCfg.endSpeed));
            }
            else
            {
                pathAssignmentVel.size = PathCfg.maxSpeed;
            }
            setPidCalMode(PID_X_Y_YAW);
            setPidTargetPose(&Pose_Adjust,PID_X_Y_YAW);
//            Vehicle_point_Now = Pose_Adjust;
        }

        if (pathFinishFlag) {
            Chassis_Sense_Bool.Forced_Close_Path_OK = true;
//            Once = true;
//            printf("Successful_Close_PAth$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$4\r\n");
            pathState = FINISH;
            pathVel.size = 0;
            pathVel.dir = 0;
        }
        else
        {
            pathState = GOING;
        }
        pathVel = pathAssignmentVel;

    }
    else
    {

        goPathID = pathID;
        PercentNow = 0.0f;
        pathState = NEW_PATH;
//        printf("NewPath###############################################################\r\n");
        pathFinishFlag = false;
//        Once = true;
        //初始化

        line_dir = atan2f(PathCfg.endPose.wy - PathCfg.startPose.wy, PathCfg.endPose.wx - PathCfg.startPose.wx);

        vertical_dir = line_dir + PI / 2;

        line_k = tanf(line_dir);

        verticle_k = tanf(vertical_dir);

        line_ver_k = line_k - verticle_k;

        b1 = PathCfg.endPose.wy - line_k * PathCfg.endPose.wx;

        Target_Line.x = PathCfg.endPose.wx - PathCfg.startPose.wx;
        Target_Line.y = PathCfg.endPose.wy - PathCfg.startPose.wy;
        Distance_Line = sqrt(square(PathCfg.endPose.wx - PathCfg.startPose.wx) + square(PathCfg.endPose.wy - PathCfg.startPose.wy));

        pathAssignmentVel.dir =  Radian2Angle(line_dir);

    }
    return pathState;
}



///*********************************************************
// * 函数名称：goStraightLine
// * 输入：起始位姿和结束位姿
// * 功能：静态跟踪直线路径
// * 参数：无
// * 输出：_Vector 运动的总速度角度
// * 返回值：路径运动状态
// * *******************************************************/
//
//static PATH_STATE GoStraightLine()       // 类型
//{
//    // 垂线角度
//    static float vertical_angle = 0.f;
//    //垂线斜率
//    static float vertical_k = 0.f;
//    // 直线角度
//    static float line_angle = 0.f;
//    // 直线斜率
//    static float line_k = 0.f;
//    // 直线与其斜线的差值
//    static float line_ver_k = 0.f;
//    // 直线
//    static float b1 = 0.f;
//    // 垂线
//    static float b2 = 0.f;
//    // 目标直线向量
//    static point targetLineVector = {0.f};
//    // 当前机器人位置到起始点向量
//    static point currentPoseToStartPoseVector = {0.f};
//    // 当前点到起始点与目标直线的投影
//    static float projectionOnTargetLineVector = 0.f;
//    // 直线长度
//    static float distanceAll = 0.f;
//    // 路径分配的速度
//    static vector pathAssignmentVel = {0.f};
//    // 调节点
//    static Pose adjustPose = {0};
//    // 路径百分比
//    static float percentNow = 0.f;
//    // 路径百分比是否达到百分百
//    static bool pathFinishFlag = false;
//    /*************************************************/
//    path_percentNow = percentNow;
//    // 判断是否更换过路径
//    if (goPathID == pathID)
//    {
//        if (!pathFinishFlag)
//        {
//            // 计算当前位置占总路程的百分比(当前位置到起始位置的向量与目标直线向量的投影 与 目标直线长度的比值)
//            currentPoseToStartPoseVector.x = Robot_Pose.wx - PathCfg.startPose.wx;
//            currentPoseToStartPoseVector.y = Robot_Pose.wy - PathCfg.startPose.wy;
//
//            projectionOnTargetLineVector = (currentPoseToStartPoseVector.x * targetLineVector.x + currentPoseToStartPoseVector.y * targetLineVector.y) / distanceAll;
//
//            percentNow = projectionOnTargetLineVector * 100 / distanceAll;
//
//            if (percentNow < 0)
//            {
////                printf("EEEEEEEEEEEEEEEEEEERORRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRR\r\n");
////                percentNow = 0.f;
////                adjustPose = PathCfg.startPose;
//            }
//            else if (percentNow > 99.f)
//            {
//                percentNow = 100.f;
//                adjustPose = PathCfg.endPose;
//                pathFinishFlag = true;
//            }
//            else if(Chassis_Sense_Bool.Forced_Close_Path)
//            {
////                printf("Rec_ForceClose!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\r\n");
//                Chassis_Sense_Bool.Forced_Close_Path = false;
//                Chassis_Sense_Bool.Forced_Close_Path_OK = false;
//                percentNow = 100.f;
//                pathFinishFlag = true;
//            }
//            else
//            {
////                printf("No_REc!!!!!!!!!!!!!!!!!!!\r\n");
//                b2 = Robot_Pose.wy - vertical_k * Robot_Pose.wx;
//                // 找到调节点
//                adjustPose.wx = (b2  - b1) / line_ver_k;
//                adjustPose.wy = (b2 * line_k - b1 * vertical_k) /line_ver_k;
//                if (percentNow <= PathCfg.yawAdjustPercent)
//                {
//                    printf("Pathh:::::StartPose.Yaw:%f    EndPose.YAw:%f   Yaw_Adjust_PErcent:%d  Output:%f \r\n",PathCfg.startPose.yaw,PathCfg.endPose.yaw,PathCfg.yawAdjustPercent,adjustPose.yaw);
//
//                    adjustPose.yaw = (PathCfg.endPose.yaw - PathCfg.startPose.yaw) * percentNow / (PathCfg.yawAdjustPercent) + PathCfg.startPose.yaw;
//                }
//                else
//                {
//                    printf("Pathh::::::::EndPose:%f::::::::::::::::::::::::::::::::::::::::::::::::::\r\n",PathCfg.endPose.yaw);
//
//                    adjustPose.yaw = PathCfg.endPose.yaw;
//                }
//            }
//        }
//
//        //分配路径速度(速度位移公式)
//        if (percentNow <= PathCfg.acceleratePercent && PathCfg.acceleratePercent != 0)
//        {
//            pathAssignmentVel.size = sqrtf((float)(percentNow / (PathCfg.acceleratePercent)) * (square(PathCfg.maxSpeed) - square(PathCfg.startSpeed))
//                         + square(PathCfg.startSpeed));
//        }
//
//        else if (100 - percentNow <= PathCfg.descentPercent && PathCfg.descentPercent != 0)
//        {
//            pathAssignmentVel.size = sqrtf((float)((100 - percentNow) / PathCfg.descentPercent) * (square(PathCfg.maxSpeed) - square(PathCfg.endSpeed))
//                         + square(PathCfg.endSpeed));
//        }
//        else
//        {
//            pathAssignmentVel.size = PathCfg.maxSpeed;
//        }
//
//        setPidTargetPose(&adjustPose,PID_X_Y_YAW);
//
//        if (pathFinishFlag) {
//            pathState = FINISH;
//            Chassis_Sense_Bool.Forced_Close_Path_OK = true;
//            printf("Successful_Close_PAth$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$4\r\n");
//        }
//        else
//        {
////            printf("GOOOINGGGGGGGG!!!!!!!!!!!\r\n");
//            pathState = GOING;
//        }
//        pathVel = pathAssignmentVel;
//    }
//    else
//    {
//        goPathID = pathID;
//        percentNow = 0.0f;
//        pathState = NEW_PATH;
//        printf("NewPath###############################################################\r\n");
//        pathFinishFlag = false;
//        // 初始化
//        line_angle = atan2f(PathCfg.endPose.wy - PathCfg.startPose.wy, PathCfg.endPose.wx - PathCfg.startPose.wx);
//
//        vertical_angle = line_angle + PI/2;
//
//        line_k = tanf(line_angle);
//
//        vertical_k = tanf(vertical_angle);
//
//        line_ver_k = line_k-vertical_k;
//
//        b1 = PathCfg.endPose.wy - line_k * PathCfg.endPose.wx;
//
//        targetLineVector.x = PathCfg.endPose.wx - PathCfg.startPose.wx;
//        targetLineVector.y = PathCfg.endPose.wy - PathCfg.startPose.wy;
//        distanceAll =sqrt(square(PathCfg.endPose.wx - PathCfg.startPose.wx) + square(PathCfg.endPose.wy - PathCfg.startPose.wy));
//        pathAssignmentVel.dir = line_angle * 180 / PI;
//
//    }
//    return pathState;
//}
/*********************************************************
 * 函数名称：B样条解算
 * 输入：
 * 功能：解算B样条权重和坐标
 * 参数：k     次
 *       n     控制点个数
 *       t     0<=t<=1
 *       knots  节点
 *       P     控制点
 *       Bij   基函数
 * 输出：
 * 返回值：B样条结果坐标
 * *******************************************************/

static float(*B_Spline_Bik(uint8_t k, uint8_t n, float t, float* knots))[32] //均匀B样条基函数
{
    uint8_t m = k + n + 1;
    static float bij[32][32] = { 0 };   //基函数
    static float At, Bt; //系数Bi,k-1前的系数
    for (uint8_t j = 0; j <= k; j++)
    {
        for (uint8_t i = 0; i < m; i++)
        {
            if (knots[i] <= t && t <= knots[i + 1]) //ti~ti+1
            {
                bij[i][0] = 1;
                if (j != 0)
                {
                    for (uint8_t ii = 0; ii < m; ii++)
                    {
                        At = (t - knots[ii]) / (knots[ii + j] - knots[ii]);
                        Bt = (knots[ii + j + 1] - t) / (knots[ii + j + 1] - knots[ii + 1]);
                        if ((knots[ii + j] - knots[ii]) == 0)               //判断分母是否为0
                            At = 0;
                        if ((knots[ii + j + 1] - knots[ii + 1]) == 0)
                            Bt = 0;
                        bij[ii][j] = bij[ii][j - 1] * At + bij[ii + 1][j - 1] * Bt;
                    }
                }
                bij[i][0] = 0;
            }
        }
    }//基函数求解完成
    return bij;
}
/**
 * @brief       B样条返回点函数
 * @param P     控制点
 * @param Bij   基函数
 * @param k     次
 * @param n     控制点个数
 * @return
*/
static point B_Spline_Point_Return(Pose P[32], float(*Bij)[32], uint8_t k, uint8_t n)
{
    point Pt = { 0 };
    for (uint8_t i = 0; i <= n; i++)
    {
        Pt.x += P[i].wx * Bij[i][k];
        Pt.y += P[i].wy * Bij[i][k];
    }
    return Pt;
}

static bool PATH_GenerateBSplinePath_4ControlPoint(BsplinePath* bsplinePath,const float knots[8], uint8_t k, uint8_t n)
{
    Ni_k ni_k = { 0.f };

    float t[UPRE_PATH_PATHNUM];
    float num = UPRE_PATH_PATHNUM;

    uint8_t m = k + n + 1;

    int i, j;

    uint8_t d = 0;
    uint8_t dd = 0;

    static float At, Bt;


    for (i = 0; (float)i / num <= 1; i++)
    {
        t[i] = (float)i / num + 0.f;
    }

    for (i = 0; i <= UPRE_PATH_PATHNUM-1 ; i++)
    {
        for (j = 0; j <= k; j++)
        {
            for (d = 0; d < m; d++)
            {
                if (knots[d] <= t[i] && t[i] <= knots[d + 1])
                {
                    ni_k.N[d][0] = 1;
                    if (j != 0)
                    {
                        for (dd = 0; dd < m - j; dd++)
                        {
                            At = (t[i] - knots[dd]) / (knots[dd + j] - knots[dd]);
                            Bt = (knots[dd + j + 1] - t[i]) / (knots[dd + j + 1] - knots[dd + 1]);
                            if ((knots[dd + j] - knots[dd]) == 0)
                            {
                                At = 0;
                            }
                            if ((knots[dd + j + 1] - knots[dd + 1]) == 0)
                            {
                                Bt = 0;
                            }
                            ni_k.N[dd][j] = ni_k.N[dd][j - 1] * At + ni_k.N[dd + 1][j - 1] * Bt;
//                            printf("N%d_%d:%f\n", d, j, ni_k.N[dd][j]);
                        }
                    }
                    ni_k.N[d][0] = 0;
                }
            }
        }
        Path_PathPoint[i].x = (bsplinePath->ctrlPoint1.wx * ni_k.N[0][3] + bsplinePath->ctrlPoint2.x * ni_k.N[1][3] + bsplinePath->ctrlPoint3.x * ni_k.N[2][3] + bsplinePath->ctrlPoint4.wx * ni_k.N[3][3]);
        Path_PathPoint[i].y = (bsplinePath->ctrlPoint1.wy * ni_k.N[0][3] + bsplinePath->ctrlPoint2.y * ni_k.N[1][3] + bsplinePath->ctrlPoint3.y * ni_k.N[2][3] + bsplinePath->ctrlPoint4.wy * ni_k.N[3][3]);
//        printf("i:%d Path_PathPoint_x:%f Path_PathPoint_y:%f\r\n",i,Path_PathPoint[i].x,Path_PathPoint[i].y);
    }



    return true;
}


/*********************************************************
 * 函数名称：B_Spline_Path  B样条曲线路径
 * 输入：起始位姿和结束位姿
 * 功能：静态跟踪直线路径
 * 参数：   ControlPoint[32]   控制点
 *          Knots               节点
 *          k                   次
 *          n                   控制点的个数-1
 * 输出：_Vector 运动的总速度角度
 * 返回值：路径运动状态
 * *******************************************************/
static PATH_STATE B_Spline_Path()
{
    //当前点
    static int32_t T0 = 0;
    //当前点的下一个点
    static int32_t T1 = 0;
    //极小的变化量
    static int32_t DeltaT = 1;
    //PID调节点
    static Pose AdjustPose;
    //当前点
    static point CurrentPoint;
    //下一点
    static point NextPoint;
    // 路径百分比
    static float PercentNow = 0.f;
    // 路径百分比是否达到百分百
    static bool pathFinishFlag = false;

    /*      N:机器人当前位置      A:参考点                        D:PID点
            B:参考点的下一个点    C:机器人当前点到切线的垂足      k为AC/AB         */
    static point LineAN;
    static point LineAB;
    static float k;

    //PID点的速度方向
    static float FinePointSpeedDirection;
    //路径分配速度
    static vector pathAssignmentVel;


    if (goPathID == pathID)
    {
        if (!pathFinishFlag)
        {
            CurrentPoint = Path_PathPoint[T0];
            if (T0 + DeltaT < UPRE_PATH_PATHNUM)
            {
                NextPoint = Path_PathPoint[T0 + DeltaT];
                LineAN.x = Robot_Pose.wx - CurrentPoint.x;
                LineAN.y = Robot_Pose.wy - CurrentPoint.y;
                LineAB.x = NextPoint.x - CurrentPoint.x;
                LineAB.y = NextPoint.y - CurrentPoint.y;

                k = (LineAN.x * LineAB.x + LineAN.y * LineAB.y) / (square(LineAB.x) + square(LineAB.y));
                T1 = T0 + k * DeltaT;
                // 判断T1是否越界
                if (T1 >= UPRE_PATH_PATHNUM)
                {
                    T1 = UPRE_PATH_PATHNUM;
                    pathFinishFlag = true;
                }
                else if (T1 < 0)
                {
                    T1 = 0;
                }
            }
            else
            {
                T1 = UPRE_PATH_PATHNUM;
                pathFinishFlag = true;
            }

            PercentNow = ((float)( T0 + 1) / (float)UPRE_PATH_PATHNUM) * 100;

            T0 = T1;
            //找到调节点
            AdjustPose.wx = Path_PathPoint[T1].x;
            AdjustPose.wy = Path_PathPoint[T1].y;
            if (PercentNow <= PathCfg.acceleratePercent)
            {
                AdjustPose.yaw = (PathCfg.endPose.yaw - PathCfg.startPose.yaw) * PercentNow / (PathCfg.yawAdjustPercent) + PathCfg.startPose.yaw;

            }
            else
            {
                AdjustPose.yaw = PathCfg.endPose.yaw;
            }
        }
        //寻找速度方向
        if (T1 == UPRE_PATH_PATHNUM)
        {
            FinePointSpeedDirection = Radian2Angle(atan2f((Path_PathPoint[T1].y - Path_PathPoint[T1 - 1].y), (Path_PathPoint[T1].x - Path_PathPoint[T1 - 1].x)));
        }
        else if (T1 == 0)
        {
            FinePointSpeedDirection = Radian2Angle(atan2f((Path_PathPoint[T1 + 1].y - Path_PathPoint[T1].y), (Path_PathPoint[T1 + 1].x - Path_PathPoint[T1].x)));
        }
        else
        {
            FinePointSpeedDirection = Radian2Angle(atan2f((Path_PathPoint[T1 + 1].y - Path_PathPoint[T1 - 1].y), (Path_PathPoint[T1 + 1].x - Path_PathPoint[T1 - 1].x)));
        }
        pathAssignmentVel.dir = FinePointSpeedDirection;

        //分配路径速度
        if (PercentNow <= PathCfg.acceleratePercent && PathCfg.acceleratePercent != 0)
        {
            pathAssignmentVel.size = sqrtf( (float)(PercentNow / PathCfg.acceleratePercent * (square(PathCfg.maxSpeed) - square(PathCfg.startSpeed)) + square(PathCfg.startSpeed) ));

        }
        else if (100 - PercentNow <= PathCfg.descentPercent && PathCfg.descentPercent != 0)
        {
            pathAssignmentVel.size = sqrtf((float)((100 - PercentNow) / PathCfg.descentPercent * (square(PathCfg.maxSpeed) - square(PathCfg.endSpeed)) + square(PathCfg.endSpeed) ));

        }
        else
        {
            pathAssignmentVel.size = PathCfg.maxSpeed;
        }

        setPidTargetPose(&AdjustPose, PID_X_Y_YAW);

        if (pathFinishFlag)
        {
            pathState = FINISH;
        }
        else
        {
            pathState = GOING;
        }
        pathVel = pathAssignmentVel;

#if Printf_BPointNow == 1
        printf("percent:%f\n", PercentNow);
        printf("BPointX:%f BPointY:%f\n", Path_PathPoint[T1].x, Path_PathPoint[T1].y);
        printf("Direction:%f\n", FinePointSpeedDirection);
        printf("T0:%d\n", T0);
#endif
    }
    else
    {
        goPathID = pathID;
        PercentNow = 0.0f;
        T0 = 0;
        pathFinishFlag = false;

        pathState = NEW_PATH;
    }
    return pathState;
}
/**********************************************************************************************************
 * 函数名称 :StraigthLinePathInit
 * 功能: 直线路径初始化设置
 * 参数: StraightLinePath 结构体，bool 是否使用默认参数，int32_t 启始速度 ，int32_t 最大速度 ， int32_t 结束速动 ，
 * uint8_t 加速百分比 ,uint8_t 减速百分比 ，uint8_t 旋转百分比 , int32_t 减速距离 ,uint32_t ,速度PID  KP,KI, KD ,积分限制幅
 * 返回值：bool
 ***********************************************************************************************************/

bool StraigthLinePathInit(const StraightLinePath* path, bool isDefaultParam,
                          int32_t startSpeed, int32_t maxSpeed, int32_t endSpeed,
                          uint8_t acceleratePercent, uint8_t descentPercent, uint8_t yawAdjustPercent)
{

    bool result = false;
    PathCfg.startPose = path->startPose;
    PathCfg.endPose = path->endPose;

    if (isDefaultParam)
    {
        PathCfg.startSpeed = DEFAULT_START_SPEED;
        PathCfg.endSpeed = DEFAULT_END_SPEED;
        PathCfg.maxSpeed = DEFAULT_MAX_SPEED;
        PathCfg.acceleratePercent = DEFAULT_ACCELERATE_PER;
        PathCfg.descentPercent = DEFAULT_DESCEND_PER;
        PathCfg.yawAdjustPercent = DEFAULT_YAW_ADJUST_PER;
        PathCfg.pathType = STRAIGHT_LINE;
        pathID++;
        setPidCalMode(PID_X_Y_YAW);
        result = true;
    }
    else
    {

        PathCfg.startSpeed = startSpeed;
        PathCfg.maxSpeed = maxSpeed;
        PathCfg.endSpeed = endSpeed;
        PathCfg.acceleratePercent = acceleratePercent;
        PathCfg.descentPercent = descentPercent;
        PathCfg.yawAdjustPercent = yawAdjustPercent;
        PathCfg.pathType = STRAIGHT_LINE;
        pathID++;
        setPidCalMode(PID_X_Y_YAW);
        result = true;

    }
    return result;
}
//设置路径平移速度,自选速度大小,一般用来清零
void setPathVel(float velSize, float pathVelDir)
{
    pathVel.size = velSize;
    pathVel.dir = pathVelDir;
}
//返回路径平移速度
vector getPathVel(void) {


    return pathVel;
}
//返回路径状态
PATH_STATE getPathState(void)
{
    return pathState;
}
PathParam pathconfig(void)
{
    return PathCfg;
}


/**********************************************************************************************************
 * 函数名称 :bspinePathInit
 * 功能: B样条路径初始化设置
 * 参数: StraightLinePath 结构体/四个点生成B样条曲线/，bool 是否使用默认参数，int32_t 启始速度 ，int32_t 最大速度 ， int32_t 结束速动 ，
 * uint8_t 加速百分比 ,uint8_t 减速百分比 ，uint8_t 旋转百分比 uint32_t ,速度PID  KP,KI, KD ,积分限制幅
 * 返回值：bool
 ***********************************************************************************************************/
bool B_SpLinePathInit(const B_Spline_PathCfg* path, bool isDefaultParam,
                      int32_t startSpeed, int32_t maxSpeed, int32_t endSpeed,
                      uint8_t acceleratePercent, uint8_t descentPercent, uint8_t yawAdjustPercent)
{
    bool result = false;
    static float t = 0.f;
    for (int32_t i = 0; i <= 1000; i++)
    {
        Path_PathPoint[i] = B_Spline_Point_Return(path->Control_Point, B_Spline_Bik(path->k, path->n, t, path->knots), path->k, path->n);
#if Printf_BPoint == 1
        printf("%f %f\n", Path_PathPoint[i].x, Path_PathPoint[i].y);
#endif
        t += 0.001;
    }
    // 检查路B样条控制点
    if (isDefaultParam)
    {
        PathCfg.startPose = path->Control_Point[0];
        PathCfg.endPose = path->Control_Point[path->n];
        PathCfg.startSpeed = DEFAULT_START_SPEED;
        PathCfg.maxSpeed = DEFAULT_MAX_SPEED;
        PathCfg.endSpeed = DEFAULT_END_SPEED;
        PathCfg.acceleratePercent = DEFAULT_ACCELERATE_PER;
        PathCfg.descentPercent = DEFAULT_DESCEND_PER;
        PathCfg.yawAdjustPercent = DEFAULT_YAW_ADJUST_PER;
        PathCfg.pathType = BSPLINE;
        pathID++;
        setPidCalMode(PID_X_Y_YAW);
        result = true;
    }
    else
    {
        PathCfg.startPose = path->Control_Point[0];
        PathCfg.endPose = path->Control_Point[path->n];
        PathCfg.startSpeed = startSpeed;
        PathCfg.maxSpeed = maxSpeed;
        PathCfg.endSpeed = endSpeed;
        PathCfg.acceleratePercent = acceleratePercent;
        PathCfg.descentPercent = descentPercent;
        PathCfg.yawAdjustPercent = yawAdjustPercent;
        //PathCfg.decelerationDistance = decelerationDistance;
        PathCfg.pathType = BSPLINE;
        //PathCfg.pathFinishDisp = pathFinishDisp;
        pathID++;
        setPidCalMode(PID_X_Y_YAW);
        result = true;
    }
    return result;
}

/**********************************************************************************************************
 * 函数名称 :bspinePathInit_4ControlPoint
 * 功能: B样条路径初始化设置
 * 参数: StraightLinePath 结构体/四个点生成B样条曲线/，bool 是否使用默认参数，int32_t 启始速度 ，int32_t 最大速度 ， int32_t 结束速动 ，
 * uint8_t 加速百分比 ,uint8_t 减速百分比 ，uint8_t 旋转百分比 uint32_t ,速度PID  KP,KI, KD ,积分限制幅
 * 返回值：bool
 ***********************************************************************************************************/
bool PATH_B_SpLinePathInit_4ControlPoint(BsplinePath* bsplinePath, bool isDefaultParam,
                                         int32_t startSpeed, int32_t maxSpeed, int32_t endSpeed,
                                         uint8_t acceleratePercent, uint8_t descentPercent, uint8_t yawAdjustPercent)
{
    bool result = false;
    uint8_t k = 3, n = 3;
    k = 3;
    n = 3;
    float u[8] = { 0, 0 , 0 , 0 , 1 , 1 , 1 , 1 };
    PATH_GenerateBSplinePath_4ControlPoint(bsplinePath,u,k,n);
    // 检查路B样条控制点
    if (isDefaultParam)
    {
        PathCfg.startPose = bsplinePath->ctrlPoint1;
        PathCfg.endPose = bsplinePath->ctrlPoint4;
        PathCfg.startSpeed = DEFAULT_START_SPEED;
        PathCfg.maxSpeed = DEFAULT_MAX_SPEED;
        PathCfg.endSpeed = DEFAULT_END_SPEED;
        PathCfg.acceleratePercent = DEFAULT_ACCELERATE_PER;
        PathCfg.descentPercent = DEFAULT_DESCEND_PER;
        PathCfg.yawAdjustPercent = DEFAULT_YAW_ADJUST_PER;
        PathCfg.pathType = BESSEL_LINE;
        pathID++;
        setPidCalMode(PID_X_Y_YAW);
        result = true;
    }
    else
    {
        PathCfg.startPose = bsplinePath->ctrlPoint1;
        PathCfg.endPose = bsplinePath->ctrlPoint4;
        PathCfg.startSpeed = startSpeed;
        PathCfg.maxSpeed = maxSpeed;
        PathCfg.endSpeed = endSpeed;
        PathCfg.acceleratePercent = acceleratePercent;
        PathCfg.descentPercent = descentPercent;
        PathCfg.yawAdjustPercent = yawAdjustPercent;
        //PathCfg.decelerationDistance = decelerationDistance;
        PathCfg.pathType = BESSEL_LINE;
        //PathCfg.pathFinishDisp = pathFinishDisp;
        pathID++;
        setPidCalMode(PID_X_Y_YAW);
        result = true;
    }
    return result;
}

// 车旋转优化
float optimizeRobRotation(float targetYaw)
{

    //float destTargetYaw;
    float deltaYaw = Angle2SameCycle360(targetYaw, Robot_Pose.yaw);
    return deltaYaw;
}
/****************************************
 * 函数名称：Path_PathTrack
 * 功能：路径追踪
 * 输入：无
 * 参数：使用全局变量 Path_PathPoint[UPRE_PATH_PATHNUM]
 * 输出：无
 * 返回值：路径运行状态
 * ************************************/
static PATH_STATE Path_PathTrack()
{
    static uint16_t T0 = 0;
    static uint16_t DeltaT = 1;
    static int16_t T1 = 0;
    // AN向量与在AB向量上的投影与AB模的比值
    float  DistanceProportionalityCoefficient = 0.f;
    // 当前路径索引点
    static point currentPathPoint = { 0 };
    // 当前路径点索引下一点
    static point NextPathPoint = { 0 };
    // AB向量
    static point ABVector = { 0 };
    // AN向量
    static point ANVector = { 0 };
    // AN向量与AB向量的点乘
    float VectorANMultipliedByVectorAB = 0.f;
    // AB向量平方
    float TheMagnitudeOfTheVectorABSquared = 0.f;
    // 路径速度方向
    float FindPointSpeedDirection = 0.f;
    // 路径分配的速度
    static vector pathAssignmentVel = { 0.f };
    // 调节点
    static Pose  adjustPose = { 0 };
    // 路径百分比
    static float percentNow = 0.f;
    // 路径百分比是否达到百分百
    static bool pathFinishFlag = false;
    // 路径卡死处理点
    static point ErrorPathPoint = {0};
    // 判断是否更换过路径
    if (goPathID == pathID)
    {
        if (!pathFinishFlag)
        {
            // 寻找调节点
            currentPathPoint = Path_PathPoint[T0];
            if ((T0 + DeltaT) < UPRE_PATH_PATHNUM)
            {
                NextPathPoint = Path_PathPoint[T0 + DeltaT];

                ABVector.x = NextPathPoint.x - currentPathPoint.x;
                ABVector.y = NextPathPoint.y - currentPathPoint.y;

                ANVector.x = Robot_Pose.wx - currentPathPoint.x;
                ANVector.y = Robot_Pose.wy - currentPathPoint.y;

                VectorANMultipliedByVectorAB = ABVector.x * ANVector.x + ABVector.y * ANVector.y;
                TheMagnitudeOfTheVectorABSquared = ABVector.x * ABVector.x + ABVector.y * ABVector.y;
                // AN向量在AB向量上的投影与AB模的比值
                DistanceProportionalityCoefficient = (float)VectorANMultipliedByVectorAB / (float)TheMagnitudeOfTheVectorABSquared;
                // T1 = T0 + K * DeltaT
                T1 = T0 + DistanceProportionalityCoefficient * DeltaT;

                // 判断T1是否越界
                if (T1 >= UPRE_PATH_PATHNUM)
                {
                    T1 = UPRE_PATH_PATHNUM - 1;
//                    pathFinishFlag = true;
                }
                else if (T1 < 0)
                {
                    T1 = 0;
                }
            }
            else
            {
                T1 = UPRE_PATH_PATHNUM - 1;
//                pathFinishFlag = true;

            }

//            printf("%f %f\n", Path_PathPoint[T0].x, Path_PathPoint[T0].y);

            percentNow = ((float)(T1 + 1) / (float)UPRE_PATH_PATHNUM) * 100;
            T0 = T1;

            // 找到调节点
            adjustPose.wx = Path_PathPoint[T1].x;
            adjustPose.wy = Path_PathPoint[T1].y;
            if (percentNow <= PathCfg.yawAdjustPercent)
            {
                adjustPose.yaw = (PathCfg.endPose.yaw - PathCfg.startPose.yaw) * percentNow / (PathCfg.yawAdjustPercent) + PathCfg.startPose.yaw;
            }
            else
            {
                adjustPose.yaw = PathCfg.endPose.yaw;
            }
        }



        if (T1 == UPRE_PATH_PATHNUM - 1)
        {
            FindPointSpeedDirection = atan2f(Path_PathPoint[T1].y - Path_PathPoint[T1 - 1].y, Path_PathPoint[T1].x - Path_PathPoint[T1 - 1].x);
        }
        else if (T1 == 0)
        {
            FindPointSpeedDirection = atan2f(Path_PathPoint[T1 + 1].y - Path_PathPoint[T1].y, Path_PathPoint[T1 + 1].x - Path_PathPoint[T1].x);
        }
        else
        {
            FindPointSpeedDirection = atan2f(Path_PathPoint[T1 + 1].y - Path_PathPoint[T1 - 1].y, Path_PathPoint[T1 + 1].x - Path_PathPoint[T1 - 1].x);
        }
        pathAssignmentVel.dir = FindPointSpeedDirection * 180.f / PI;

        // 分配路径速度
        if (percentNow <= PathCfg.acceleratePercent && PathCfg.acceleratePercent != 0)
        {
            pathAssignmentVel.size = sqrt((float)(percentNow / (PathCfg.acceleratePercent)) * (square(PathCfg.maxSpeed) - square(PathCfg.startSpeed))
                                          + square(PathCfg.startSpeed));
        }
        else if (100 - percentNow <= PathCfg.descentPercent && PathCfg.descentPercent != 0)
        {
            pathAssignmentVel.size = sqrt((float)((100 - percentNow) / PathCfg.descentPercent) * (square(PathCfg.maxSpeed) - square(PathCfg.endSpeed))
                                          + square(PathCfg.endSpeed));
        }
        else
        {
            pathAssignmentVel.size = PathCfg.maxSpeed;
        }


#if Printf_Percent_Bezier == 1
        printf("buf_x:%f\tbuf_y:%f\tpercent:%f\r\n",location.buf_x,location.buf_y,percentNow);
#endif
        if(Chassis_Sense_Bool.Flag_ReachZone3)
        {
            if(location.buf_x == 0.f && location.buf_y == 0.f && percentNow >=70.f)
            {
//                percentNow = 100.f;
                PathCfg.pathFinishDisp = FINISH;
                pathFinishFlag = true;
            }
        }
        // 求出速度路径速度方向

        if(percentNow >= 100.f) {
            PathCfg.pathFinishDisp = FINISH;
            pathFinishFlag = true;

        }

        ErrorPathPoint = currentPathPoint;
        setPidTargetPose(&adjustPose,PID_X_Y_YAW);

        path_percentNow = percentNow;
        /**********************************************************************************解决路径卡死问题****************************************************************************************************************************/
        /***************************************************************************************************************************************************************************************************************************/

        if (pathFinishFlag)
        {
            switch (PathCfg.pathFinishDisp) {

                case FINISH:
                {
                    pathAssignmentVel.size = 0.f;
//                    setPidCalMode(PID_OFF);       //TODO
                    pathState = FINISH;
                    break;
                }
                case NEW_PATH:
                {
                    setPidCalMode(PID_YAW_ONLY);
                    setPidTargetPose(&PathCfg.endPose,PID_X_Y_YAW);
                    break;
                }

                case STOP:
                {
                    setPidTargetPose(&PathCfg.endPose,PID_X_Y_YAW);
                    break;
                }

                default:
                {
                    pathAssignmentVel.size = 0.f;
                    setPidCalMode(PID_OFF);
                    break;
                }
            }

            pathState = FINISH;
        }
        else
        {
            pathState = GOING;
        }

        pathVel = pathAssignmentVel;
    }
    else
    {
        goPathID = pathID;
        percentNow = 0.0f;
        T0 = 0;
        pathFinishFlag = false;
        pathState = NEW_PATH;
    }
    return pathState;
}
/****************************************
 * 函数名称：checkBesselPathPoint
 * 功能：检查路径点是否完成赋值
 * 输入：路径类型 PathCfg.pathType
 * 参数：无
 * 输出：无
 * 返回值：路径运行状态
 * ************************************/
static bool Path_checkPathPoint()
{
    for (int i = 1; i < UPRE_PATH_PATHNUM - 1; i++)
    {
        if ((int)Path_PathPoint[i - 1].x | (int)Path_PathPoint[i].x == 0 &&
            (int)Path_PathPoint[i - 1].y | (int)Path_PathPoint[i].y == 0 &&
            (int)Path_PathPoint[i].x | (int)Path_PathPoint[i + 1].x == 0 &&
            (int)Path_PathPoint[i].y | (int)Path_PathPoint[i + 1].y == 0)
        {
            return false;
        }
    }
    return true;
}

/****************************************
 * 函数名称：Path_GeneratBessel
 * 功能：生成三阶四控制点贝塞尔曲线路径
 * 输入：路径类型 PathCfg.pathType
 * 参数：无
 * 输出：无
 * 返回值：路径运行状态
 * ************************************/
static bool Path_GeneratBessel(PATH_BesselControlPoint *PATH_BesselControlPoint)
{

    const float Path_DivideEqually = UPRE_PATH_DivideEqually;
    for (int i = 0; i < UPRE_PATH_PATHNUM; i++)
    {
        static float t = 0;
        t = i * Path_DivideEqually;
        PATH_BesselControlPoint->Path_BesselTransitionPoint[0].x = (1 - t) * PATH_BesselControlPoint->Path_BesselPoint_1.wx + t * PATH_BesselControlPoint->Path_BesselPoint_2.x;
        PATH_BesselControlPoint->Path_BesselTransitionPoint[0].y = (1 - t) * PATH_BesselControlPoint->Path_BesselPoint_1.wy + t * PATH_BesselControlPoint->Path_BesselPoint_2.y;

        PATH_BesselControlPoint->Path_BesselTransitionPoint[1].x = (1 - t) * PATH_BesselControlPoint->Path_BesselPoint_2.x + t * PATH_BesselControlPoint->Path_BesselPoint_3.x;
        PATH_BesselControlPoint->Path_BesselTransitionPoint[1].y = (1 - t) * PATH_BesselControlPoint->Path_BesselPoint_2.y + t * PATH_BesselControlPoint->Path_BesselPoint_3.y;

        PATH_BesselControlPoint->Path_BesselTransitionPoint[2].x = (1 - t) * PATH_BesselControlPoint->Path_BesselPoint_3.x + t * PATH_BesselControlPoint->Path_BesselPoint_4.wx;
        PATH_BesselControlPoint->Path_BesselTransitionPoint[2].y = (1 - t) * PATH_BesselControlPoint->Path_BesselPoint_3.y + t * PATH_BesselControlPoint->Path_BesselPoint_4.wy;

        PATH_BesselControlPoint->Path_BesselTransitionPoint[3].x = (1 - t) * PATH_BesselControlPoint->Path_BesselTransitionPoint[0].x + t * PATH_BesselControlPoint->Path_BesselTransitionPoint[1].x;
        PATH_BesselControlPoint->Path_BesselTransitionPoint[3].y = (1 - t) * PATH_BesselControlPoint->Path_BesselTransitionPoint[0].y + t * PATH_BesselControlPoint->Path_BesselTransitionPoint[1].y;

        PATH_BesselControlPoint->Path_BesselTransitionPoint[4].x = (1 - t) * PATH_BesselControlPoint->Path_BesselTransitionPoint[1].x + t * PATH_BesselControlPoint->Path_BesselTransitionPoint[2].x;
        PATH_BesselControlPoint->Path_BesselTransitionPoint[4].y = (1 - t) * PATH_BesselControlPoint->Path_BesselTransitionPoint[1].y + t * PATH_BesselControlPoint->Path_BesselTransitionPoint[2].y;

        Path_PathPoint[i].x = (1 - t) * PATH_BesselControlPoint->Path_BesselTransitionPoint[3].x + t * PATH_BesselControlPoint->Path_BesselTransitionPoint[4].x;
        Path_PathPoint[i].y = (1 - t) * PATH_BesselControlPoint->Path_BesselTransitionPoint[3].y + t * PATH_BesselControlPoint->Path_BesselTransitionPoint[4].y;
//        printf("%d:%f, %f, %f\n", i, t, Path_PathPoint[i].x, Path_PathPoint[i].y);
    }
//    if (Path_checkPathPoint(PATH_BesselControlPoint))
//    {
//        return true;
//    }
    return false;
}

/**********************************************************************************************************
 * 函数名称 :Path_BesselPathInit
 * 功能: 贝塞尔曲线初始化
 * 参数: PATH_BesselControlPoint 结构体/四个点生成贝塞尔曲线/，bool 是否使用默认参数，int32_t 启始速度 ，int32_t 最大速度 ， int32_t 结束速动 ，
 * uint8_t 加速百分比 ,uint8_t 减速百分比 ，uint8_t 旋转百分比 uint32_t ,速度PID  KP,KI, KD ,积分限制幅
 * 返回值：bool
 ***********************************************************************************************************/
Pose Print_Stright_Pose = {0};
bool Path_BesselPathInit(PATH_BesselControlPoint *Path_BesselControlPoint, bool isDefaultParam,
                         int32_t startSpeed, int32_t maxSpeed, int32_t endSpeed,
                         uint8_t acceleratePercent, uint8_t descentPercent, uint8_t yawAdjustPercent)
{
    Print_Stright_Pose = Path_BesselControlPoint->Path_BesselPoint_4;
    bool result = false;
    static float t = 0.f;
    Path_GeneratBessel(Path_BesselControlPoint);
    // 检查路B样条控制点
    if (isDefaultParam)
    {
        PathCfg.startPose = Path_BesselControlPoint->Path_BesselPoint_1;
        PathCfg.endPose = Path_BesselControlPoint->Path_BesselPoint_4;
        PathCfg.startSpeed = DEFAULT_START_SPEED;
        PathCfg.maxSpeed = DEFAULT_MAX_SPEED;
        PathCfg.endSpeed = DEFAULT_END_SPEED;
        PathCfg.acceleratePercent = DEFAULT_ACCELERATE_PER;

        PathCfg.descentPercent = DEFAULT_DESCEND_PER;
        PathCfg.yawAdjustPercent = DEFAULT_YAW_ADJUST_PER;
        PathCfg.pathType = BESSEL_LINE;
        pathID++;
        setPidCalMode(PID_X_Y_YAW);
        result = true;
    }
    else
    {
        PathCfg.startPose =Path_BesselControlPoint->Path_BesselPoint_1;
        PathCfg.endPose = Path_BesselControlPoint->Path_BesselPoint_4;
        PathCfg.startSpeed = startSpeed;
        PathCfg.maxSpeed = maxSpeed;
        PathCfg.endSpeed = endSpeed;
        PathCfg.acceleratePercent = acceleratePercent;
        PathCfg.descentPercent = descentPercent;
        PathCfg.yawAdjustPercent = yawAdjustPercent;
        //PathCfg.decelerationDistance = decelerationDistance;
        PathCfg.pathType = BESSEL_LINE;
        //PathCfg.pathFinishDisp = pathFinishDisp;
        pathID++;
        setPidCalMode(PID_X_Y_YAW);
        result = true;
    }
    return result;
}

/****************************************
 * 函数名称：goPath
 * 功能：根据初始化选择要运行的路径类型
 * 输入：路径类型 PathCfg.pathType
 * 参数：无
 * 输出：无
 * 返回值：路径运行状态
 * ************************************/
PATH_STATE goPath()
{
#if Printf_PathState == 1
    printf("PathState:%d\tPathType:%d", pathState,PathCfg.pathType);
#endif
    path_PathState = pathState;
    path_type = PathCfg.pathType;
    static uint8_t State = 0;
    //根据初始化的方式来选择路径
    switch (PathCfg.pathType)
    {
        case STRAIGHT_LINE:
        {
            State = GoStraightLine();
            break;
        }
        case BSPLINE:
            State = B_Spline_Path();
            break;
        case BESSEL_LINE:
            State = Path_PathTrack();
        default:
        {
            pathState = PATH_ERROR;
            break;
        }

    }
    pathState = State;
    return pathState;
}

